#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from geometry_msgs.msg import TransformStamped
import math
from transforms3d.euler import quat2euler

class RobotPoseListener(Node):
    def __init__(self):
        super().__init__('robot_pose_listener')
        
        # 创建 tf buffer 和 listener
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        
        # 创建定时器，每秒获取一次机器人位置
        self.timer = self.create_timer(1.0, self.get_robot_pose)

    def get_robot_pose(self):
        try:
            # 获取机器人相对于地图的变换
            transform: TransformStamped = self.tf_buffer.lookup_transform(
                'map',
                'base_link',
                rclpy.time.Time())
            
            # 获取四元数
            q = transform.transform.rotation
            # 转换为欧拉角
            roll, pitch, yaw = quat2euler([q.w, q.x, q.y, q.z])
            # 将偏航角转换为角度，并调整到0-360度范围
            yaw_deg = math.degrees(yaw)
            if yaw_deg < 0:
                yaw_deg += 360
            
            # 打印位置信息
            print(f'机器人位置: x={transform.transform.translation.x:.2f}, '
                  f'y={transform.transform.translation.y:.2f}, '
                  f'z={transform.transform.translation.z:.2f}, '
                  f'航向角={yaw_deg:.2f}°')
            
        except TransformException as ex:
            self.get_logger().warn(f'无法获取变换: {str(ex)}')

def main():
    rclpy.init()
    node = RobotPoseListener()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
